#ifndef __ROB_H__
#define __ROB_H__

#include "common.h"
#include "mfl.h"  /* Include EASML Math Float Library */

/* Robot Arm Configuration */
#define ROB_SERVO_NUM 6u
#define ROB_DOF 6u
#define ROB_PWM_PERIOD 20000u
#define ROB_PWM_MIN 500u
#define ROB_PWM_MAX 2500u
#define ROB_PWM_CENTER 1500u

/* Robot Arm Link Configuration Structure */
typedef struct {
    float32 L0;  /* Base height */
    float32 L1;  /* First link length */
    float32 L2;  /* Second link length */
    float32 L3;  /* End effector length */
} Rob_LinkConfig_Type;

/* Robot Joint Angles Structure */
typedef struct {
    float32 Theta[ROB_DOF];  /* Joint angles in degrees */
} Rob_JointAngles_Type;

/* Robot Cartesian Position Structure */
typedef struct {
    float32 X;      /* X position */
    float32 Y;      /* Y position */
    float32 Z;      /* Z position */
    float32 Alpha;  /* End effector pitch angle */
    float32 Beta;   /* End effector roll angle */
} Rob_CartesianPos_Type;

/* Robot Servo Control Structure */
typedef struct {
    uint16 PwmValue[ROB_SERVO_NUM];  /* PWM values */
    float32 CurrentAngle[ROB_SERVO_NUM];  /* Current angles */
    float32 TargetAngle[ROB_SERVO_NUM];   /* Target angles */
    uint16 MoveTime[ROB_SERVO_NUM];       /* Movement time */
    boolean IsMoving[ROB_SERVO_NUM];      /* Movement status */
} Rob_ServoControl_Type;

/* Robot System State Structure */
typedef struct {
    Rob_LinkConfig_Type LinkConfig;
    Rob_JointAngles_Type CurrentAngles;
    Rob_JointAngles_Type TargetAngles;
    Rob_CartesianPos_Type CurrentPos;
    Rob_CartesianPos_Type TargetPos;
    Rob_ServoControl_Type ServoControl;
    Mfl_StatePID_Type PidStates[ROB_SERVO_NUM];  /* PID controllers for each servo */
    Mfl_ParamPID_Type PidParams[ROB_SERVO_NUM];  /* PID parameters */
} Rob_SystemState_Type;

/* Function Declarations */

/* Initialization Functions */
void Rob_Init(Rob_SystemState_Type *State_pst, 
              float32 L0_f32, float32 L1_f32, float32 L2_f32, float32 L3_f32);

/* Kinematics Functions */
uint8 Rob_InverseKinematics(float32 X_f32, float32 Y_f32, float32 Z_f32, 
                           float32 Alpha_f32, Rob_SystemState_Type *State_pst);

void Rob_ForwardKinematics(const Rob_JointAngles_Type *Angles_cpst,
                          const Rob_LinkConfig_Type *Config_cpst,
                          Rob_CartesianPos_Type *Position_pst);

/* Servo Control Functions */
void Rob_SetServoAngle(Rob_SystemState_Type *State_pst, uint8 ServoIndex_u8, 
                       float32 Angle_f32, uint16 Time_u16);

void Rob_SetAllServoAngles(Rob_SystemState_Type *State_pst, 
                          const float32 Angles_pf32[ROB_SERVO_NUM], uint16 Time_u16);

uint16 Rob_AngleToPwm(float32 Angle_f32, uint8 ServoIndex_u8);

float32 Rob_PwmToAngle(uint16 PwmValue_u16, uint8 ServoIndex_u8);

/* Movement Functions */
void Rob_MoveToPosition(Rob_SystemState_Type *State_pst, 
                       float32 X_f32, float32 Y_f32, float32 Z_f32, 
                       float32 Alpha_f32, uint16 Time_u16);

boolean Rob_IsMovementComplete(const Rob_SystemState_Type *State_cpst);

/* PID Control Functions */
void Rob_InitPID(Rob_SystemState_Type *State_pst, uint8 ServoIndex_u8,
                float32 Kp_f32, float32 Ki_f32, float32 Kd_f32);

void Rob_UpdatePID(Rob_SystemState_Type *State_pst, uint8 ServoIndex_u8, 
                  float32 Setpoint_f32, float32 Feedback_f32, float32 dT_f32);

/* Utility Functions */
float32 Rob_DegToRad(float32 Degrees_f32);
float32 Rob_RadToDeg(float32 Radians_f32);
float32 Rob_LimitAngle(float32 Angle_f32, float32 Min_f32, float32 Max_f32);

#endif /* __ROB_H__ */
